#include "sfm_basic.h"

namespace ville3d {
Eigen::Matrix4d View::getInvPose() const {
  Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
  pose.block<3, 3>(0, 0) = quats_.toRotationMatrix();
  pose.block<3, 1>(0, 3) = trans_;
  pose = pose.inverse();
  return pose;
}

Eigen::Matrix4d View::getPose() const {
  Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
  pose.block<3, 3>(0, 0) = quats_.toRotationMatrix();
  pose.block<3, 1>(0, 3) = trans_;
  return pose;
}

Eigen::Vector3d View::getCameraCenter() const { return trans_; }

bool View::project(const Eigen::Vector3d& X, Eigen::Vector2d& uv) const {
  Eigen::Vector3d local_coord =
      getInvPose().block<3, 4>(0, 0) * X.homogeneous();
  if (local_coord.z() <= 0) {
    return false;
  }
  Eigen::Vector3d uvw = intrinsic_ * local_coord;
  uvw /= uvw.z();
  uv = uvw.head(2);
  if (uv.x() < 0 || uv.x() > width) {
    return false;
  }
  if (uv.y() < 0 || uv.y() > height) {
    return false;
  }
  return true;
}

bool View::cheiralityTest(const Eigen::Vector2d& uv, const Eigen::Vector3d& X) {
  Eigen::Vector3d local_point =
      getInvPose().block<3, 4>(0, 0) * X.homogeneous();
  Eigen::Vector3d bearing = intrinsic_.inverse() * uv.homogeneous();
  return bearing.dot(local_point) > 0.0;
}

Eigen::Vector2d View::residual(const Eigen::Vector3d& X,
                               const Eigen::Vector2d& x) const {
  Eigen::Vector2d proj;
  project(X, proj);
  return x - proj;
}

std::vector<ville3d::IndexT> View::getFeaturesInArea(const float& x,
                                                     const float& y,
                                                     const float& r) {
  std::vector<ville3d::IndexT> indexes;
  for (std::size_t i = 0; i < key_points_.size(); i++) {
    cv::KeyPoint key_pt = key_points_[i];
    float squar_dis = (key_pt.pt.x - x) * (key_pt.pt.x - x) +
                      (key_pt.pt.y - y) * (key_pt.pt.y - y);
    if (squar_dis < r * r) {
      indexes.push_back(i);
    }
  }
  return indexes;
}

MapPoint::MapPoint(Eigen::Vector3d position, TrackID track_id)
    : position_(position), track_id_(track_id_), obs_num(0) {}

bool TrackSet::search_feat_by_track_image_id(const TrackID& track_id,
                                             const IndexT& view_idx,
                                             IndexT& feat_idx) {
  std::vector<std::pair<IndexT, IndexT>>& img_feats = tracks[track_id];
  auto find_it = std::find_if(img_feats.begin(), img_feats.end(),
                              [=](std::pair<IndexT, IndexT> img_feat) -> bool {
                                return img_feat.first == view_idx;
                              });
  if (find_it != img_feats.end()) {
    feat_idx = find_it->second;
    return true;
  }
  return false;
}

bool TrackSet::remove_image(const IndexT& view_idx) {
  if (image_tracks.find(view_idx) == image_tracks.end()) {
    return false;
  }
  std::set<TrackID> track_ids = image_tracks[view_idx];
  for (TrackID track_id : track_ids) {
    remove_image_from_track(track_id, view_idx);
  }
  image_tracks.erase(view_idx);
  return true;
}

bool TrackSet::remove_track(const TrackID& track_id) {
  if (tracks.find(track_id) == tracks.end()) {
    return false;
  }
  std::vector<std::pair<IndexT, IndexT>> img_feats = tracks[track_id];
  for (auto img_feat : img_feats) {
    IndexT view_idx = img_feat.first;
    image_tracks[view_idx].erase(track_id);
    imageFeat_tracks.erase(img_feat);
  }
  tracks.erase(track_id);
  return true;
}

bool TrackSet::remove_image_from_track(const TrackID& track_id,
                                       const IndexT& view_idx) {
  if (tracks.find(track_id) == tracks.end()) {
    return false;
  }
  std::vector<std::pair<IndexT, IndexT>>& img_feats = tracks[track_id];
  for (std::size_t i = 0; i < img_feats.size(); i++) {
    imageFeat_tracks.erase(img_feats[i]);
  }
  img_feats.erase(
      std::remove_if(img_feats.begin(), img_feats.end(),
                     [=](std::pair<IndexT, IndexT> img_feat) -> bool {
                       return img_feat.first == view_idx;
                     }),
      img_feats.end());
  image_tracks[view_idx].erase(track_id);
  return true;
}

}  // namespace ville3d
